//Debug data
#define DEBUG 0
#define BAUD 38400

// Define pin assignments
#define LEDPIN 13
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5

// Define transmitter commands
#define THROTTLETIMEOUT 25000
#define PULSETIMEOUT 4500
int throttle = 0;
int throttleMotorCommand, yawMotorCommand, pitchMotorCommand, rollMotorCommand;
int roll = 0;
int pitch = 0;
int yaw = 0;
unsigned long previousTime = 0;
unsigned long deltaTime = 0;
unsigned int frontCommand, rightCommand, leftCommand, rearCommand;

void setup() {
  pinMode(LEDPIN, OUTPUT);
  digitalWrite(LEDPIN, HIGH);
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  Serial.begin(BAUD);
  for (byte channel = 0; channel < 4; channel++) motorConfigure(channel);
  delay(5000);
}

void loop() {
  // Read transmitter commands
  previousTime = millis();
  roll = pulseIn(ROLLPIN, HIGH, THROTTLETIMEOUT);
  pitch = pulseIn(PITCHPIN, HIGH, PULSETIMEOUT);
  throttle = pulseIn(THROTTLEPIN, HIGH, PULSETIMEOUT);
  yaw = pulseIn(YAWPIN, HIGH, PULSETIMEOUT);
  deltaTime = millis() - previousTime;
  
  // Calculate motor commands
  // change 1000-2000 range to 2000-4700
  // Range for Pololu RC servo controller is 2000-4700
  throttleMotorCommand = (constrain((throttle - 1000), 0, 1000) * 2.7) + 2000;
  yawMotorCommand = (constrain((yaw - 1000), 0, 1000) * 2.7) + 2000;
  pitchMotorCommand = (constrain((pitch - 1000), 0, 1000) * 2.7) + 2000;
  rollMotorCommand = (constrain((roll - 1000), 0, 1000) * 2.7) + 2000;
/*   
  frontCommand = (unsigned int)constrain(throttleMotorCommand + pitchMotorCommand - yawMotorCommand, 2000, 4700);
  rearCommand = (unsigned int)constrain(throttleMotorCommand - pitchMotorCommand - yawMotorCommand, 2000, 4700);
  rightCommand = (unsigned int)constrain(throttleMotorCommand + rollMotorCommand + yawMotorCommand, 2000, 4700);
  leftCommand = (unsigned int)constrain(throttleMotorCommand - rollMotorCommand + yawMotorCommand, 2000, 4700);
*/  
  frontCommand = (unsigned int)constrain(throttleMotorCommand, 2000, 4700);
  rearCommand = (unsigned int)constrain(rollMotorCommand, 2000, 4700);
  rightCommand = (unsigned int)constrain(pitchMotorCommand, 2000, 4700);
  leftCommand = (unsigned int)constrain(yawMotorCommand, 2000, 4700);
  
  motorCommand(0, frontCommand);  
  motorCommand(1, rightCommand);  
  motorCommand(2, rearCommand);  
  motorCommand(3, leftCommand);  
  
  if (DEBUG) {
    digitalWrite(LEDPIN, HIGH);
    Serial.print(deltaTime);
    Serial.print(",");
    Serial.print(throttleMotorCommand);
    Serial.print(",");
    Serial.print(rollMotorCommand);
    Serial.print(",");
    Serial.print(pitchMotorCommand);
    Serial.print(",");
    Serial.print(yawMotorCommand);
    Serial.print(",");
    Serial.println(yaw);
    digitalWrite(LEDPIN, LOW);  
  }  
}

void motorCommand(byte servo, unsigned int angle){
   //servo is the servo number (typically 0-7)
   //angle is the absolute position from 2000 to 4695?

   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x04,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   //Convert the angle data into two 7-bit bytes
   Serial.print(((angle>>7)&0x3f),BYTE); //data1
   Serial.print((angle&0x7f),BYTE); //data2
}

void motorConfigure(byte servo){
   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x01,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   Serial.print(0x0,BYTE); //instant change
   
   motorCommand(servo, 2000);
}
